/****************************************************************************
 *
 *   Copyright (C) 2016 James Y. Wilson. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "MPU9250.hpp"
#include "MPU9250_mag.hpp"

using namespace DriverFramework;

int MPU9250_mag::initialize()
{
    int result = 0;
    int8_t retries = 5;

    result = _imu.modifyReg(MPUREG_USER_CTRL, 0, BITS_USER_CTRL_I2C_MST_EN, true);

    if (result != 0) {
        DF_LOG_ERR("AK8963: IMU I2C master bus enable failed.");
        return -1;
    }

    result = _imu.writeReg(MPUREG_I2C_MST_CTRL, BITS_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_P_NSR | BITS_I2C_MST_CLK_400KHZ);

	if (result != 0) {
        DF_LOG_ERR("AK8963: IMU I2C master bus config failed.");
		return -1;
	}

    //try to bring up mag
    for (int i=0; i<retries; i++) {
        result = passthrough_write_verified(MPU9250_MAG_REG_CNTL2, BIT_MAG_CNTL2_SOFT_RESET);

        if (result == 0) {
            break;
        } else {
            // Reset i2c master.
            _imu.modifyReg(MPUREG_USER_CTRL, 0, BITS_USER_CTRL_I2C_MST_RST);
        }

        DF_LOG_ERR("AK8963: bring up mag failed #%d", i);
        usleep(1000);
    }

    if (result != 0) {
        DF_LOG_ERR("AK8963: bring up failed.");
        return -1;
    }

	// Detect mag presence by reading whoami register
    if (check_id() != 0) {
        DF_LOG_ERR("AK8963: mag not detected.");
		return -1;
	}

	// Get mag calibraion data from Fuse ROM
    if (get_sensitivity_adjustment() != 0) {
        DF_LOG_ERR("AK8963: failed to read adjustment data.");
        return -1;
    }

    passthrough_write(MPU9250_MAG_REG_CNTL1, BIT_MAG_CNTL1_MODE_CONTINUOUS_MEASURE_MODE_2 | BIT_MAG_CNTL1_16_BITS);
    usleep(1000);
    set_passthrough(MPU9250_MAG_REG_ST1, sizeof(struct mag_data));

    return 0;
}

int MPU9250_mag::process(const struct mag_data &data, float &mag_ga_x, float &mag_ga_y, float &mag_ga_z)
{
    // Check magnetic sensor overflow HOFL bit set.  No need to check the data ready bit, since
    // the sample rate divider should provide new samples at the correct interval.
    if (data.mag_st2 & BIT_MAG_HOFL) {
        return MAG_ERROR_DATA_OVERFLOW;
    }

    mag_ga_x = data.mag_x * _mag_sens_adj[0] * MAG_RAW_TO_GAUSS;
    mag_ga_y = data.mag_y * _mag_sens_adj[1] * MAG_RAW_TO_GAUSS;
    mag_ga_z = data.mag_z * _mag_sens_adj[2] * MAG_RAW_TO_GAUSS;


    // Swap magnetometer x and y axis, and invert z because internal mag in MPU9250
    // has a different orientation.
    // Magnetometer X axis = Gyro and Accel Y axis
    // Magnetometer Y axis = Gyro and Accel X axis
    // Magnetometer Z axis = -Gyro and Accel Z axis
    float temp_mag_x = mag_ga_x;
    mag_ga_x = mag_ga_y;
    mag_ga_y = temp_mag_x;

    mag_ga_z = -mag_ga_z;

    return 0;
}

int MPU9250_mag::check_id(void)
{
    uint8_t deviceid = 0;
    passthrough_read(MPU9250_MAG_REG_WIA, &deviceid, 0x01);

    return (deviceid == MPU9250_AKM_DEV_ID ? 0 : -1);
}

int MPU9250_mag::get_sensitivity_adjustment(void)
{
    uint8_t response[3];
    float ak8963_ASA[3];

    passthrough_write(MPU9250_MAG_REG_CNTL1, BIT_MAG_CNTL1_FUSE_ROM_ACCESS_MODE | BIT_MAG_CNTL1_16_BITS);
    usleep(50);
    passthrough_read(MPU9250_MAG_REG_ASAX, response, 3);
    passthrough_write(MPU9250_MAG_REG_CNTL1, BIT_MAG_CNTL1_MODE_POWER_DOWN);

    for (int i = 0; i < 3; i++) {
        if (0 != response[i] && 0xff != response[i]) {
            ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;

        } else {
            return -1;
        }
    }

    _mag_sens_adj[0] = ak8963_ASA[0];
    _mag_sens_adj[1] = ak8963_ASA[1];
    _mag_sens_adj[2] = ak8963_ASA[2];

    DF_LOG_DEBUG("mag_sens_adj = { %f, %f, %f }",
                (double)_mag_sens_adj[0], (double)_mag_sens_adj[1], (double)_mag_sens_adj[2]);

    return 0;
}

void
MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
{
    uint8_t addr;

    _imu.writeReg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers

    if (out) {
        _imu.writeReg(MPUREG_I2C_SLV0_D0, *out);
        addr = MPU9250_AK8963_I2C_ADDR;

    } else {
        addr = MPU9250_AK8963_I2C_ADDR | MPU9250_AK8963_I2C_READ;
    }

    _imu.writeReg(MPUREG_I2C_SLV0_ADDR, addr);
    _imu.writeReg(MPUREG_I2C_SLV0_REG,  reg);
    _imu.writeReg(MPUREG_I2C_SLV0_CTRL, size | BITS_I2C_SLV0_EN);
}

void MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
{
    set_passthrough(reg, 1, &val);
    usleep(50); // wait for the value to be written to slave
    _imu.writeReg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
}

void MPU9250_mag::passthrough_read(uint8_t reg, uint8_t* buf, uint8_t size)
{
    set_passthrough(reg, size);

    usleep(50 + 50 * size); // wait for the value to be read from slave
                            // 25us used in PX, will failed some time.
    _imu.bulkReadReg(MPUREG_EXT_SENS_DATA_00, buf, size);
    _imu.writeReg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
}

int MPU9250_mag::passthrough_write_verified(uint8_t reg, uint8_t val)
{
    int retVal = 0;
    uint8_t b = 0;

    // Configure a write operation to the mag using Slave 4.
    retVal = writeRegVerified(MPUREG_I2C_SLV4_ADDR, MPU9250_AK8963_I2C_ADDR, 0xff);

    if (retVal != 0) {
        return retVal;
    }

    // Set the mag register address to write to using Slave 4.
    retVal = writeRegVerified(MPUREG_I2C_SLV4_REG, reg, 0xff);

    if (retVal != 0) {
        return retVal;
    }

    // Set the value to write in the I2C_SLV4_DO register.
    retVal = writeRegVerified(MPUREG_I2C_SLV4_DO, val, 0xff);

    if (retVal != 0) {
        return retVal;
    }

    // Read the current value of the Slave 4 control register.
    retVal = _imu.readReg(MPUREG_I2C_SLV4_CTRL, b);

    if (retVal != 0) {
        return retVal;
    }

    // Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
    // bits.
    b |= BITS_I2C_SLV4_EN;
    // Trigger the data transfer from the byte now stored in the SLV4_DO register.
    retVal = _imu.writeReg(MPUREG_I2C_SLV4_CTRL, b);

    if (retVal != 0) {
        return retVal;
    }

    // Continuously check I2C_MST_STATUS regsiter value for the completion
    // of I2C transfer until timeout.

    int loop_ctrl = 100; // wait up to 1000 * 1ms for completion

    do {
        usleep(1000);
        retVal = _imu.readReg(MPUREG_I2C_MST_STATUS, b);

        if (retVal != 0) {
            return retVal;
        }
    } while (((b & BITS_I2C_SLV4_DONE) == 0x00) && (--loop_ctrl));

    if (loop_ctrl == 0) {
        DF_LOG_ERR("AK8963: I2C transfer to mag timed out");
        return -1;
    }

    return 0;
}

int MPU9250_mag::writeRegVerified(int reg, uint8_t val, uint8_t mask)
{
    int retVal;
    uint8_t b;
    int retry = 5;
    bool err_seen;

    while (retry) {
        err_seen = false;
        --retry;
        retVal = _imu.writeReg(reg, val);

        if (retVal != 0) {
            err_seen = true;
            continue;
        }

        retVal = _imu.readReg(reg, b);

        if (retVal != 0) {
            err_seen = true;
            continue;
        }

        if ((b & mask) != val) {
            continue;

        } else {
            return 0;
        }
    }

    if (err_seen) {
        DF_LOG_ERR("AK8963: writeRegVerified failed for reg %d. Error %d.",
               reg, retVal);

    } else {
        DF_LOG_ERR("AK8963: writeRegVerified failed for reg %d. %d!=%d",
               reg, val, b);
    }

    return retVal;
}
